#include "Saber_control.hpp"

//构造函数
Saber::Saber(void)
{
    solution_num=0;
    std::array<std::array<double,6>,8> solution_list{};
    std::array<double,6> theta{};
}

uint8_t Saber::inverse_kinematics_solver(const double* target_pose)
{   
    double nx=target_pose[0];double ox=target_pose[1];double ax=target_pose[2];double px=target_pose[3];
    double ny=target_pose[4];double oy=target_pose[5];double ay=target_pose[6];double py=target_pose[7];
    double nz=target_pose[8];double oz=target_pose[9];double az=target_pose[10];double pz=target_pose[11];

    for(uint8_t i=0;i<8;i++)
    {
        solution_list[i].fill(0);
    }
    std::array<uint8_t,8> solution_flag;
    solution_flag.fill(1);

    //---theta1---
    std::array<double,2> theta1;
    double m=d6*ay-py;double n=d6*ax-px;
    // ROS_INFO("m:%f n:%f",m,n);
    if((pow(m,2)+pow(n,2)-pow(d4,2))<0)
    {
        ROS_INFO("No_solution!");
        return 0;
    }
    theta1[0]=atan2(m,n)-atan2(d4,sqrt(pow(m,2)+pow(n,2)-pow(d4,2)));
    theta1[1]=atan2(m,n)-atan2(d4,-sqrt(pow(m,2)+pow(n,2)-pow(d4,2)));
    ROS_INFO("theta1(1):%f theta1(2):%f",theta1[0],theta1[1]);
    //---theta5---
    std::array<std::array<double,2>,2> theta5;
    theta5[0][0]=acos(ax*sin(theta1[0])-ay*cos(theta1[0]));
    theta5[0][1]=acos(ax*sin(theta1[1])-ay*cos(theta1[1]));
    theta5[1][0]=-acos(ax*sin(theta1[0])-ay*cos(theta1[0]));
    theta5[1][1]=-acos(ax*sin(theta1[1])-ay*cos(theta1[1]));
    ROS_INFO("theta5[0][0]:%f theta5[0][1]:%f",theta5[0][0],theta5[0][1]);
    ROS_INFO("theta5[1][0]:%f theta5[1][1]:%f",theta5[1][0],theta5[1][1]);

    //---theta6---
    std::array<std::array<double,2>,2> theta6;
    theta6[0].fill(0);
    theta6[1].fill(0);
    std::array<double,2> mm;std::array<double,2> nn;
    mm[0]=nx*sin(theta1[0])-ny*cos(theta1[0]);mm[1]=nx*sin(theta1[1])-ny*cos(theta1[1]);
    nn[0]=ox*sin(theta1[0])-oy*cos(theta1[0]);nn[1]=ox*sin(theta1[1])-oy*cos(theta1[1]);
    // ROS_INFO("mm[0]:%f mm[1]:%f",mm[0],mm[1]);
    // ROS_INFO("nn[0]:%f nn[1]:%f",nn[0],nn[1]);
    theta6[0][0]=atan2(mm[0],nn[0])-atan2(sin(theta5[0][0]),0);theta6[0][1]=atan2(mm[1],nn[1])-atan2(sin(theta5[0][1]),0);
    theta6[1][0]=atan2(mm[0],nn[0])-atan2(sin(theta5[1][0]),0);theta6[1][1]=atan2(mm[1],nn[1])-atan2(sin(theta5[1][1]),0);
    ROS_INFO("theta6[0][0]:%f theta6[0][1]:%f",theta6[0][0],theta6[0][1]);
    ROS_INFO("theta6[1][0]:%f theta6[1][1]:%f",theta6[1][0],theta6[1][1]);
    
    //---theta3---
    std::array<std::array<double,2>,4> theta3;
    std::array<std::array<double,2>,2> theta3_pre;
    std::array<std::array<double,2>,2> mmm;std::array<std::array<double,2>,2> nnn;
    for(uint8_t i=0;i<2;i++)
    {
        for(uint8_t j=0;j<2;j++)
        {
            mmm[i][j]=d5*(sin(theta6[i][j])*(nx*cos(theta1[j])+ny*sin(theta1[j]))+cos(theta6[i][j])*(ox*cos(theta1[j])+oy*sin(theta1[j])))-d6*(ax*cos(theta1[j])+ay*sin(theta1[j]))+px*cos(theta1[j])+py*sin(theta1[j]);
        }

    }
    for(uint8_t i=0;i<2;i++)
    {
        for(uint8_t j=0;j<2;j++)
        {
            nnn[i][j]=pz-d1-az*d6+d5*(oz*cos(theta6[i][j])+nz*sin(theta6[i][j]));
        }

    }
    for(uint8_t i=0;i<2;i++)
    {
        for(uint8_t j=0;j<2;j++)
        {
            theta3_pre[i][j]=(pow(mmm[i][j],2)+pow(nnn[i][j],2)-pow(a2,2)-pow(a3,2))/(2*a2*a3);
        }

    }
    if(abs(theta3_pre[0][0])>1){theta3_pre[0][0]=0;solution_flag[0]=0;solution_flag[1]=0;}
    if(abs(theta3_pre[0][1])>1){theta3_pre[0][1]=0;solution_flag[4]=0;solution_flag[5]=0;}
    if(abs(theta3_pre[1][0])>1){theta3_pre[1][0]=0;solution_flag[2]=0;solution_flag[3]=0;}
    if(abs(theta3_pre[1][1])>1){theta3_pre[1][1]=0;solution_flag[6]=0;solution_flag[7]=0;}
    for(uint8_t i=0;i<4;i++)
    {
        for(uint8_t j=0;j<2;j++)
        {
            if(i<2){theta3[i][j]=acos(theta3_pre[i][j]);}
            else {theta3[i][j]=-acos(theta3_pre[i-2][j]);};
        }

    }
    // ROS_INFO("mmm[0][0]:%f mmm[0][1]:%f",mmm[0][0],mmm[0][1]);
    // ROS_INFO("mmm[1][0]:%f mmm[1][1]:%f",mmm[1][0],mmm[1][1]);
    // ROS_INFO("nnn[0][0]:%f nnn[0][1]:%f",nnn[0][0],nnn[0][1]);
    // ROS_INFO("nnn[1][0]:%f nnn[1][1]:%f",nnn[1][0],nnn[1][1]);
    // ROS_INFO("theta3_pre[0][0]:%f theta3_pre[0][1]:%f",theta3_pre[0][0],theta3_pre[0][1]);
    // ROS_INFO("theta3_pre[1][0]:%f theta3_pre[1][1]:%f",theta3_pre[1][0],theta3_pre[1][1]);
    ROS_INFO("theta3[0][0]:%f theta3[0][1]:%f",theta3[0][0],theta3[0][1]);
    ROS_INFO("theta3[1][0]:%f theta3[1][1]:%f",theta3[1][0],theta3[1][1]);
    ROS_INFO("theta3[2][0]:%f theta3[2][1]:%f",theta3[2][0],theta3[2][1]);
    ROS_INFO("theta3[3][0]:%f theta3[3][1]:%f",theta3[3][0],theta3[3][1]);

    //---theta2---
    std::array<std::array<double,2>,4> theta2;
    std::array<std::array<double,2>,4> mmm_2;std::array<std::array<double,2>,4> nnn_2;
    std::array<std::array<double,2>,4> s2;std::array<std::array<double,2>,4> c2;
    for(uint8_t i=0;i<4;i++)
    {
        for(uint8_t j=0;j<2;j++)
        {
            if(i<2){mmm_2[i][j]=mmm[i][j];}
            else {mmm_2[i][j]=mmm[i-2][j];};
        }

    }
    for(uint8_t i=0;i<4;i++)
    {
        for(uint8_t j=0;j<2;j++)
        {
            if(i<2){nnn_2[i][j]=nnn[i][j];}
            else {nnn_2[i][j]=nnn[i-2][j];};
        }

    }
    for(uint8_t i=0;i<4;i++)
    {
        for(uint8_t j=0;j<2;j++)
        {
            s2[i][j]=((a3*cos(theta3[i][j])+a2)*nnn_2[i][j]-a3*sin(theta3[i][j])*mmm_2[i][j])/ (pow(a2,2)+pow(a3,2)+2*a2*a3*cos(theta3[i][j]));
        }
    }
    for(uint8_t i=0;i<4;i++)
    {
        for(uint8_t j=0;j<2;j++)
        {
            c2[i][j]=(mmm_2[i][j]+a3*sin(theta3[i][j])*s2[i][j])/(a3*cos(theta3[i][j])+a2);
        }
    }
    for(uint8_t i=0;i<4;i++)
    {
        for(uint8_t j=0;j<2;j++)
        {
            theta2[i][j]=atan2(s2[i][j],c2[i][j]);
        }
    }
    // ROS_INFO("s2[0][0]:%f s2[0][1]:%f",s2[0][0],s2[0][1]);
    // ROS_INFO("s2[1][0]:%f s2[1][1]:%f",s2[1][0],s2[1][1]);
    // ROS_INFO("s2[2][0]:%f s2[2][1]:%f",s2[2][0],s2[2][1]);
    // ROS_INFO("s2[3][0]:%f s2[3][1]:%f",s2[3][0],s2[3][1]);
    // ROS_INFO("c2[0][0]:%f c2[0][1]:%f",c2[0][0],c2[0][1]);
    // ROS_INFO("c2[1][0]:%f c2[1][1]:%f",c2[1][0],c2[1][1]);
    // ROS_INFO("c2[2][0]:%f c2[2][1]:%f",c2[2][0],c2[2][1]);
    // ROS_INFO("c2[3][0]:%f c2[3][1]:%f",c2[3][0],c2[3][1]);
    ROS_INFO("theta2[0][0]:%f theta2[0][1]:%f",theta2[0][0],theta2[0][1]);
    ROS_INFO("theta2[1][0]:%f theta2[1][1]:%f",theta2[1][0],theta2[1][1]);
    ROS_INFO("theta2[2][0]:%f theta2[2][1]:%f",theta2[2][0],theta2[2][1]);
    ROS_INFO("theta2[3][0]:%f theta2[3][1]:%f",theta2[3][0],theta2[3][1]);

    //整理关节角 1 5 6 3 2
    std::array<std::array<double,6>,8> theta;
    //整理theta1
    for(uint8_t i=0;i<8;i++)
    {
            if(i<4){theta[i][0]=theta1[0];}
            else {theta[i][0]=theta1[1];};
    }
    //整理theta2
    theta[0][1]=theta2[0][0];theta[1][1]=theta2[2][0];theta[2][1]=theta2[1][0];theta[3][1]=theta2[3][0];theta[4][1]=theta2[0][1];theta[5][1]=theta2[2][1];theta[6][1]=theta2[1][1];theta[7][1]=theta2[3][1];
    //整理theta3
    theta[0][2]=theta3[0][0];theta[1][2]=theta3[2][0];theta[2][2]=theta3[1][0];theta[3][2]=theta3[3][0];theta[4][2]=theta3[0][1];theta[5][2]=theta3[2][1];theta[6][2]=theta3[1][1];theta[7][2]=theta3[3][1];
    //整理theta5
    theta[0][4]=theta5[0][0];theta[1][4]=theta5[0][0];theta[2][4]=theta5[1][0];theta[3][4]=theta5[1][0];theta[4][4]=theta5[0][1];theta[5][4]=theta5[0][1];theta[6][4]=theta5[1][1];theta[7][4]=theta5[1][1];
    //整理theta6
    theta[0][5]=theta6[0][0];theta[1][5]=theta6[0][0];theta[2][5]=theta6[1][0];theta[3][5]=theta6[1][0];theta[4][5]=theta6[0][1];theta[5][5]=theta6[0][1];theta[6][5]=theta6[1][1];theta[7][5]=theta6[1][1];

    //---theta4---
    for(uint8_t i=0;i<8;i++)
    {
        theta[i][3]=atan2(-sin(theta[i][5])*(nx*cos(theta[i][0])+ny*sin(theta[i][0]))-cos(theta[i][5])*(ox*cos(theta[i][0])+oy*sin(theta[i][0])),oz*cos(theta[i][5])+nz*sin(theta[i][5]))-theta[i][1]-theta[i][2];
    }
    
    //整理所有存在的解
    solution_num=0;
    for(uint8_t i=0;i<8;i++)
    {
        if(solution_flag[i]==1)
        {
            for(uint8_t j=0;j<6;j++)
            {
                if(j==1) solution_list[solution_num][j]=angle_adjust_2PI(theta[i][j]);
                else     solution_list[solution_num][j]=angle_adjust_PI(theta[i][j]);
            }
            solution_num++;
        }
    }
    if(solution_num==0)
    {
        ROS_INFO("No_solution!");
        return 0;
    }
    else 
    {   ROS_INFO("--------------------solution_list--------------------");
        ROS_INFO("   1        2        3        4        5        6");
        for(uint8_t i=0;i<solution_num;i++)
        {
            ROS_INFO("%.4f   %.4f   %.4f   %.4f   %.4f   %.4f",solution_list[i][0],solution_list[i][1],solution_list[i][2],solution_list[i][3],solution_list[i][4],solution_list[i][5]);
        }
        solution_choose();
        return solution_num;
    }
}

//将角度调整到-pi到pi 针对关节角1 3 4 5 6
double Saber::angle_adjust_PI(double angle)
{
    if(abs(angle)>PI)
    {
        if(angle<0) return angle+2*PI;
        else        return angle-2*PI;
    }
    else
    {
        return angle;
    }
}

//将角度调整到0到2pi 针对关节角2
double Saber::angle_adjust_2PI(double angle)
{
    if(angle<0)
    {
        return angle+2*PI;
    }
    else
    {
        return angle;
    }
}

uint8_t Saber::solution_choose(void)
{
    for(uint8_t i=0;i<solution_num;i++)
    {
        if(abs(solution_list[i][0])<(PI/2))
        {
            if(solution_list[i][1]>0 && solution_list[i][1]< PI)
            {
                for(uint8_t j=0;j<6;j++)
                {
                    theta[j]=solution_list[i][j];
                }
                return 0;
            }
            else continue;
        }
        else continue;
    }
    return 0;
}